
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode_follow.c
  * @author     baiyang
  * @date       2023-8-9
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mode.h"
#include "fms.h"

#include <mb_follow/mb_follow.h>
#include <common/location/location.h>

#if MODE_FOLLOW_ENABLED == ENABLED
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool mfollow_init(mode_base_t mode, const bool ignore_checks);
static void mfollow_exit(mode_base_t mode);
static void mfollow_run(mode_base_t mode);
static uint32_t mfollow_wp_distance();
static int32_t mfollow_wp_bearing();
static bool mfollow_get_wp(Location *loc);
static void mfollow_limit_velocity_2D(float kP, float accel_cmss, Vector2f_t *desired_vel_cms, const Vector2f_t *limit_direction, float limit_distance_cm, float dt);
static float mfollow_get_max_speed(float kP, float accel_cmss, float distance_cm, float dt);
static ModeNumber mfollow_mode_number(mode_base_t mode);
static bool mfollow_requires_GPS(mode_base_t mode);
static bool mfollow_has_manual_throttle(mode_base_t mode);
static bool mfollow_allows_arming(mode_base_t mode, enum ArmingMethod method);
static bool mfollow_is_autopilot(mode_base_t mode);
static const char *mfollow_name(mode_base_t mode);
static const char *mfollow_name4(mode_base_t mode);
/*----------------------------------variable----------------------------------*/
static struct mode_ops mode_follow_ops = {
        .mode_number = mfollow_mode_number,
        .init        = mfollow_init,
        .exit        = mfollow_exit,
        .run         = mfollow_run,
        .requires_GPS = mfollow_requires_GPS,
        .has_manual_throttle = mfollow_has_manual_throttle,
        .allows_arming = mfollow_allows_arming,
        .is_autopilot = mfollow_is_autopilot,
        .has_user_takeoff = NULL,
        .in_guided_mode = NULL,
        .logs_attitude = NULL,
        .allows_save_trim = NULL,
        .allows_autotune = NULL,
        .allows_flip = NULL,
        .name = mfollow_name,
        .name4 = mfollow_name4,
        .is_taking_off = NULL,
        .is_landing = NULL,
        .requires_terrain_failsafe = NULL,
        .get_wp = NULL,
        .wp_bearing = NULL,
        .wp_distance = NULL,
        .crosstrack_error = NULL,
        .output_to_motors = NULL,
        .use_pilot_yaw = NULL,
        .throttle_hover = NULL,
        .do_user_takeoff_start = NULL,
        .set_speed_xy = NULL,
        .set_speed_up = NULL,
        .set_speed_down = NULL,
        .pause = NULL,
        .resume = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mode_follow_ctor(ModeFollow* mode_follow)
{
    mode_ctor(&mode_follow->mode, &mode_follow_ops);
}

/*
 * mode_follow.cpp - follow another mavlink-enabled vehicle by system id
 *
 * TODO: stick control to move around on sphere
 * TODO: stick control to change sphere diameter
 * TODO: "channel 7 option" to lock onto "pointed at" target
 * TODO: do better in terms of loitering around the moving point; may need a PID?  Maybe use loiter controller somehow?
 * TODO: extrapolate target vehicle position using its velocity and acceleration
 * TODO: ensure AC_AVOID_ENABLED is true because we rely on it velocity limiting functions
 */

// initialise follow mode
static bool mfollow_init(mode_base_t mode, const bool ignore_checks)
{
    if (!mb_follow_enabled()) {
        gcs_send_text(MAV_SEVERITY_WARNING, "Set FOLL_ENABLE = 1");
        return false;
    }

#if 0
    // follow the lead vehicle using sysid
    if (mb_follow_option_is_enabled(FOLLOW_MOUNT_MOUNT_FOLLOW_ON_ENTER)) {
        mount->set_target_sysid(g2.follow.get_target_sysid());
    }
#endif

    // re-use guided mode
    return mode_init(&fms.mode_guided.mode, ignore_checks);
}

// perform cleanup required when leaving follow mode
static void mfollow_exit(mode_base_t mode)
{
    (void)mode;
    mb_follow_clear_offsets_if_required();
}

static void mfollow_run(mode_base_t mode)
{
    ModeFollow *mode_follow = (ModeFollow *)mode;

    // if not armed set throttle to zero and exit immediately
    if (mode_is_disarmed_or_landed()) {
        mode_make_safe_ground_handling(mode, false);
        return;
    }

    // set motors to full range
    mb_motors_set_desired_spool_state(mode->motors, MB_MOTOR_DESIRED_THROTTLE_UNLIMITED);

    // re-use guided mode's velocity controller
    // Note: this is safe from interference from GCSs and companion computer's whose guided mode
    //       position and velocity requests will be ignored while the vehicle is not in guided mode

    // variables to be sent to velocity controller
    Vector3f_t desired_velocity_neu_cms = VECTOR3F_DEFAULT_VALUE;
    bool use_yaw = false;
    float yaw_cd = 0.0f;

    Vector3f_t dist_vec = VECTOR3F_DEFAULT_VALUE;  // vector to lead vehicle
    Vector3f_t dist_vec_offs = VECTOR3F_DEFAULT_VALUE;  // vector to lead vehicle + offset
    Vector3f_t vel_of_target = VECTOR3F_DEFAULT_VALUE;  // velocity of lead vehicle
    if (mb_follow_get_target_dist_and_vel_ned(&dist_vec, &dist_vec_offs, &vel_of_target)) {
        // convert dist_vec_offs to cm in NEU
        const Vector3f_t dist_vec_offs_neu = {dist_vec_offs.x * 100.0f, dist_vec_offs.y * 100.0f, -dist_vec_offs.z * 100.0f};

        // calculate desired velocity vector in cm/s in NEU
        const float kp = mb_follow_get_pos_p()->_kp;
        desired_velocity_neu_cms.x = (vel_of_target.x * 100.0f) + (dist_vec_offs_neu.x * kp);
        desired_velocity_neu_cms.y = (vel_of_target.y * 100.0f) + (dist_vec_offs_neu.y * kp);
        desired_velocity_neu_cms.z = (-vel_of_target.z * 100.0f) + (dist_vec_offs_neu.z * kp);

        // scale desired velocity to stay within horizontal speed limit
        float desired_speed_xy = math_sqrtf(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y));
        if (!math_flt_zero(desired_speed_xy) && (desired_speed_xy > posctrl_get_max_speed_xy_cms(mode->pos_control))) {
            const float scalar_xy = posctrl_get_max_speed_xy_cms(mode->pos_control) / desired_speed_xy;
            desired_velocity_neu_cms.x *= scalar_xy;
            desired_velocity_neu_cms.y *= scalar_xy;
            desired_speed_xy = posctrl_get_max_speed_xy_cms(mode->pos_control);
        }

        // limit desired velocity to be between maximum climb and descent rates
        desired_velocity_neu_cms.z = math_constrain_float(desired_velocity_neu_cms.z, -fabsf(posctrl_get_max_speed_down_cms(mode->pos_control)), posctrl_get_max_speed_up_cms(mode->pos_control));

        // unit vector towards target position (i.e. vector to lead vehicle + offset)
        Vector3f_t dir_to_target_neu = dist_vec_offs_neu;
        const float dir_to_target_neu_len = vec3_length(&dir_to_target_neu);
        if (!math_flt_zero(dir_to_target_neu_len)) {
            vec3_mult(&dir_to_target_neu, &dir_to_target_neu, 1.0f / dir_to_target_neu_len);
        }

        // create horizontal desired velocity vector (required for slow down calculations)
        Vector2f_t desired_velocity_xy_cms = {desired_velocity_neu_cms.x, desired_velocity_neu_cms.y};

        // create horizontal unit vector towards target (required for slow down calculations)
        Vector2f_t dir_to_target_xy = {desired_velocity_xy_cms.x, desired_velocity_xy_cms.y};
        if (!vec2_is_zero(&dir_to_target_xy)) {
            vec2_norm(&dir_to_target_xy, &dir_to_target_xy);
        }

        // slow down horizontally as we approach target (use 1/2 of maximum deceleration for gentle slow down)
        const float dist_to_target_xy = math_norm(dist_vec_offs_neu.x, dist_vec_offs_neu.y);
        mfollow_limit_velocity_2D(posctrl_get_pos_xy_p(mode->pos_control)->_kp, posctrl_get_max_accel_xy_cmss(mode->pos_control) * 0.5f, &desired_velocity_xy_cms, &dir_to_target_xy, dist_to_target_xy, fms.G_Dt);
        // copy horizontal velocity limits back to 3d vector
        desired_velocity_neu_cms.x = desired_velocity_xy_cms.x;
        desired_velocity_neu_cms.y = desired_velocity_xy_cms.y;

        // limit vertical desired_velocity_neu_cms to slow as we approach target (we use 1/2 of maximum deceleration for gentle slow down)
        const float des_vel_z_max = mfollow_get_max_speed(posctrl_get_pos_z_p(mode->pos_control)->_kp, posctrl_get_max_accel_z_cmss(mode->pos_control) * 0.5f, fabsf(dist_vec_offs_neu.z), fms.G_Dt);
        desired_velocity_neu_cms.z = math_constrain_float(desired_velocity_neu_cms.z, -des_vel_z_max, des_vel_z_max);

        // limit the velocity for obstacle/fence avoidance
        //copter.avoid.adjust_velocity(desired_velocity_neu_cms, pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy_cmss(), pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z_cmss(), G_Dt);

        // calculate vehicle heading
        switch (mb_follow_get_yaw_behave()) {
            case FOLLOW_YAW_BEHAVE_FACE_LEAD_VEHICLE: {
                Vector2f_t zero = VECTOR2F_DEFAULT_VALUE;
                if (vec3_length_squared_xy(&dist_vec) > 1.0) {
                    yaw_cd = vec2_get_bearing_cd(&zero, (Vector2f_t *)&dist_vec);
                    use_yaw = true;
                }
                break;
            }

            case FOLLOW_YAW_BEHAVE_SAME_AS_LEAD_VEHICLE: {
                float target_hdg = 0.0f;
                if (mb_follow_get_target_heading_deg(&target_hdg)) {
                    yaw_cd = target_hdg * 100.0f;
                    use_yaw = true;
                }
                break;
            }

            case FOLLOW_YAW_BEHAVE_DIR_OF_FLIGHT: {
                Vector2f_t zero = VECTOR2F_DEFAULT_VALUE;
                if (vec3_length_squared_xy(&desired_velocity_neu_cms) > (100.0 * 100.0)) {
                    yaw_cd = vec2_get_bearing_cd(&zero, (Vector2f_t *)&desired_velocity_neu_cms);
                    use_yaw = true;
                }
                break;
            }

            case FOLLOW_YAW_BEHAVE_NONE:
            default:
                // do nothing
               break;

        }
    }

    // log output at 10hz
    uint32_t now = time_millis();
    bool log_request = false;
    if ((now - mode_follow->last_log_ms >= 100) || (mode_follow->last_log_ms == 0)) {
        log_request = true;
        mode_follow->last_log_ms = now;
    }
    // re-use guided mode's velocity controller (takes NEU)
    mguided_set_velocity(&fms.mode_guided.mode, &desired_velocity_neu_cms, use_yaw, yaw_cd, false, 0.0f, false, log_request);

    mode_run(&fms.mode_guided.mode);
}

static uint32_t mfollow_wp_distance()
{
    return mb_follow_get_distance_to_target() * 100;
}

static int32_t mfollow_wp_bearing()
{
    return mb_follow_get_bearing_to_target() * 100;
}

/*
  get target position for mavlink reporting
 */
static bool mfollow_get_wp(Location *loc)
{
    float dist = mb_follow_get_distance_to_target();
    float bearing = mb_follow_get_bearing_to_target();
    *loc = fms.current_loc;
    location_offset_bearing(loc, bearing, dist);
    return true;
}

/*
 * Note: This method is used to limit velocity horizontally only 
 * Limits the component of desired_vel_cms in the direction of the unit vector
 * limit_direction to be at most the maximum speed permitted by the limit_distance_cm.
 *
 * Uses velocity adjustment idea from Randy's second email on this thread:
 * https://groups.google.com/forum/#!searchin/drones-discuss/obstacle/drones-discuss/QwUXz__WuqY/qo3G8iTLSJAJ
 */
static void mfollow_limit_velocity_2D(float kP, float accel_cmss, Vector2f_t *desired_vel_cms, const Vector2f_t *limit_direction, float limit_distance_cm, float dt)
{
    const float max_speed = mfollow_get_max_speed(kP, accel_cmss, limit_distance_cm, dt);
    // project onto limit direction
    const float speed = vec2_dot(desired_vel_cms, limit_direction);
    if (speed > max_speed) {
        // subtract difference between desired speed and maximum acceptable speed
        vec2_add_mult(desired_vel_cms, limit_direction, max_speed - speed);
    }
}

/*
 * Computes the speed such that the stopping distance
 * of the vehicle will be exactly the input distance.
 */
static float mfollow_get_max_speed(float kP, float accel_cmss, float distance_cm, float dt)
{
    if (math_flt_zero(kP)) {
        return math_sqrtf(2.0f * distance_cm * accel_cmss);
    } else {
        return control_sqrt_controller(distance_cm, kP, accel_cmss, dt);
    }
}

static ModeNumber mfollow_mode_number(mode_base_t mode) { return FOLLOW; }
static bool mfollow_requires_GPS(mode_base_t mode) { return true; }
static bool mfollow_has_manual_throttle(mode_base_t mode) { return false; }
static bool mfollow_allows_arming(mode_base_t mode, enum ArmingMethod method) { return false; };
static bool mfollow_is_autopilot(mode_base_t mode) { return true; }
static const char *mfollow_name(mode_base_t mode) { return "FOLLOW"; }
static const char *mfollow_name4(mode_base_t mode) { return "FOLL"; }
/*------------------------------------test------------------------------------*/
#endif

